#include <iostream>
#include "BezierIntersection.h"

using namespace std;
using namespace common;

bool BezierIntersection::CalcBezierIntersection(const BezierCurve& curve_a, const BezierCurve& curve_b, vector<Eigen::Vector3f>& intersections, float threshold) {
	threshold_ = threshold;
	intersections_.clear();
	bool isIntersect = RecursiveCalcIntersection(curve_a, curve_b);

	for (auto i : intersections_) {
		if (intersections.empty())
			intersections.push_back(i);
		bool isNear = false;
		for (auto k : intersections) {
			if ((i - k).norm() <= threshold) {
				isNear = true;
			}
		}
		if (!isNear)
			intersections.push_back(i);
	}
	return isIntersect;
}

bool BezierIntersection::RecursiveCalcIntersection(const BezierCurve& curve_a, const BezierCurve& curve_b) {
	if (curve_a.points_.size() == 0 || curve_b.points_.size() == 0)
	{
		cout << "Invalid curve input!" << endl;
		return false;
	}

	if (!common::isBoundingBoxOverlap(curve_a.points_, curve_b.points_))
		return false;

	vector<Eigen::Vector3f> mid_points_a(3);
	mid_points_a[0] = (curve_a.points_[0] + curve_a.points_[1]) / 2.0;
	mid_points_a[2] = (curve_a.points_[1] + curve_a.points_[2]) / 2.0;
	mid_points_a[1] = (mid_points_a[0] + mid_points_a[2]) / 2.0;

	vector<Eigen::Vector3f> mid_points_b(3);
	mid_points_b[0] = (curve_b.points_[0] + curve_b.points_[1]) / 2.0;
	mid_points_b[2] = (curve_b.points_[1] + curve_b.points_[2]) / 2.0;
	mid_points_b[1] = (mid_points_b[0] + mid_points_b[2]) / 2.0;

	if ((mid_points_a[1] - mid_points_b[1]).norm() <= threshold_) {
		intersections_.push_back((mid_points_a[1] + mid_points_b[1]) / 2.0);
		return true;
	}
	else {
		bool isIntersect = false;
		isIntersect |= RecursiveCalcIntersection(BezierCurve(curve_a.points_[0], mid_points_a[0], mid_points_a[1]), BezierCurve(curve_b.points_[0], mid_points_b[0], mid_points_b[1]));
		isIntersect |= RecursiveCalcIntersection(BezierCurve(curve_a.points_[0], mid_points_a[0], mid_points_a[1]), BezierCurve(mid_points_b[1], mid_points_b[2], curve_b.points_[2]));
		isIntersect |= RecursiveCalcIntersection(BezierCurve(mid_points_a[1], mid_points_a[2], curve_a.points_[2]), BezierCurve(curve_b.points_[0], mid_points_b[0], mid_points_b[1]));
		isIntersect |= RecursiveCalcIntersection(BezierCurve(mid_points_a[1], mid_points_a[2], curve_a.points_[2]), BezierCurve(mid_points_b[1], mid_points_b[2], curve_b.points_[2]));
		return isIntersect;
	}
}